#include "contiki.h"

#include "net/ip/uip.h"
#include "net/ipv6/uip-ds6.h"
#include "net/ip/uip-udp-packet.h"
#include "net/rpl/rpl.h"

#include "lib/random.h"
#include "net/netstack.h"
#include "dev/leds.h"

#include "ffz/ffz-collect.h"
#define DEBUG 1
#include "ffz/debug.h"

void collect_view_construct_message(struct collect_view_data_msg *msg,
                                    const linkaddr_t *parent,
                                    uint16_t parent_etx,
                                    uint16_t current_rtmetric,
                                    uint16_t num_neighbors,
                                    uint16_t beacon_interval)
{
    static unsigned long last_cpu, last_lpm, last_transmit, last_listen;
    unsigned long cpu, lpm, transmit, listen;

    msg->len = sizeof(struct collect_view_data_msg) / sizeof(uint16_t);
    msg->clock = clock_time();
#if TIMESYNCH_CONF_ENABLED
    msg->timesynch_time = timesynch_time();
#else  /* TIMESYNCH_CONF_ENABLED */
    msg->timesynch_time = 0;
#endif /* TIMESYNCH_CONF_ENABLED */

    energest_flush();

    cpu = energest_type_time(ENERGEST_TYPE_CPU) - last_cpu;
    lpm = energest_type_time(ENERGEST_TYPE_LPM) - last_lpm;
    transmit = energest_type_time(ENERGEST_TYPE_TRANSMIT) - last_transmit;
    listen = energest_type_time(ENERGEST_TYPE_LISTEN) - last_listen;

    /* Make sure that the values are within 16 bits. If they are larger,
     we scale them down to fit into 16 bits. */
    while (cpu >= 65536ul || lpm >= 65536ul ||
           transmit >= 65536ul || listen >= 65536ul)
    {
        cpu /= 2;
        lpm /= 2;
        transmit /= 2;
        listen /= 2;
    }

    msg->cpu = cpu;
    msg->lpm = lpm;
    msg->transmit = transmit;
    msg->listen = listen;

    last_cpu = energest_type_time(ENERGEST_TYPE_CPU);
    last_lpm = energest_type_time(ENERGEST_TYPE_LPM);
    last_transmit = energest_type_time(ENERGEST_TYPE_TRANSMIT);
    last_listen = energest_type_time(ENERGEST_TYPE_LISTEN);

    memcpy(&msg->parent, &parent->u8[LINKADDR_SIZE - 2], 2);
    msg->parent_etx = parent_etx;
    msg->current_rtmetric = current_rtmetric;
    msg->num_neighbors = num_neighbors;
    msg->beacon_interval = beacon_interval;

    // memset(msg->sensors, 0, sizeof(msg->sensors));
    // collect_view_arch_read_sensors(msg);
}

void show_construct_message(struct packet_data_msg *msg)
{
    PRINTF("struct:%u %u ",msg->seqno, msg->for_alignment);
    PRINTF("%u \n",msg->msg.parent_etx);
}

void collect_common_send(void)
{
    static uint8_t seqno;
    struct packet_data_msg msg;
    /* struct collect_neighbor *n; */
    uint16_t parent_etx;
    uint16_t rtmetric;
    uint16_t num_neighbors;
    uint16_t beacon_interval;
    rpl_parent_t *preferred_parent;
    linkaddr_t parent;
    rpl_dag_t *dag;

    // printf("sizeof(msg) = %d\n", sizeof(msg));   46
    memset(&msg, 0, sizeof(msg));
    seqno++;
    if (seqno == 0)
    {
        /* Wrap to 128 to identify restarts */
        seqno = 128;
    }
    msg.seqno = seqno;

    linkaddr_copy(&parent, &linkaddr_null);
    parent_etx = 0;

    /* Let's suppose we have only one instance */
    dag = rpl_get_any_dag();
    if (dag != NULL)
    {
        preferred_parent = dag->preferred_parent;
        if (preferred_parent != NULL)
        {
            uip_ds6_nbr_t *nbr;
            nbr = uip_ds6_nbr_lookup(rpl_get_parent_ipaddr(preferred_parent));
            if (nbr != NULL)
            {
                /* Use parts of the IPv6 address as the parent address, in reversed byte order. */
                parent.u8[LINKADDR_SIZE - 1] = nbr->ipaddr.u8[sizeof(uip_ipaddr_t) - 2];
                parent.u8[LINKADDR_SIZE - 2] = nbr->ipaddr.u8[sizeof(uip_ipaddr_t) - 1];
                parent_etx = rpl_get_parent_rank((uip_lladdr_t *)uip_ds6_nbr_get_ll(nbr)) / 2;
            }
        }
        rtmetric = dag->rank;
        beacon_interval = (uint16_t)((2L << dag->instance->dio_intcurrent) / 1000);
        num_neighbors = uip_ds6_nbr_num();
    }
    else
    {
        rtmetric = 0;
        beacon_interval = 0;
        num_neighbors = 0;
    }

    /* num_neighbors = collect_neighbor_list_num(&tc.neighbor_list); */
    collect_view_construct_message(&msg.msg, &parent,
                                   parent_etx, rtmetric,
                                   num_neighbors, beacon_interval);

    show_construct_message(&msg);
}
